#include "core.h"
#include "stdio.h"
#include <string.h>
#include <stdlib.h>

/**
 * @brief 初始化CPU
 */
struct riscv_Core *core_init(void)
{
    struct riscv_Core *RISCV;
    // 申请内存
    RISCV = (struct riscv_Core *)malloc(sizeof(struct riscv_Core));
    memset(RISCV, 0, sizeof(struct riscv_Core));
    // 初始化
    RISCV->pc = 0;
    // 初始化寄存器
    for (int i = 0; i < 32; i++)
    {
        RISCV->reg[i] = 0;
    }
    // 初始化csr
    RISCV->csr = (struct csr *)malloc(sizeof(struct csr));
    csr_init(RISCV->csr);
    // 初始化定时器
    timer_init();
    // 初始化外设
    RISCV->devices = (struct device **)malloc(sizeof(struct device *) * 256);
    dev_init(RISCV->devices);
    return RISCV;
}

/**
 * @brief 释放CPU
 */
void core_free(struct riscv_Core *RISCV)
{
    free(RISCV->csr);
    free(RISCV->devices);
    free(RISCV);
}

/**
 * @brief 陷入指令
 */
void trap(struct riscv_Core *RISCV, uint32_t mcause)
{
    uint32_t mstatus, mtvec;
    mstatus = csr_read(RISCV->csr, CSR_MSTATUS);
    mtvec = csr_read(RISCV->csr, CSR_MTVEC);
    // 设置mepc,如果是中断则设置为pc,如果是异常则设置为pc+4
    csr_write(RISCV->csr, CSR_MEPC, ((mcause & MCAUSE_INT) ? RISCV->pc : RISCV->pc + 4));
    // 设置mcause,保存中断原因
    csr_write(RISCV->csr, CSR_MCAUSE, mcause);
    // 设置mstatus,关闭中断
    mstatus &= ~MSTATUS_MIE;
    mstatus |= MSTATUS_MPIE;
    csr_write(RISCV->csr, CSR_MSTATUS, mstatus);
    // 设置pc
    if((mtvec & 0x3) == 0){
        // 直接跳转
        RISCV->pc = mtvec;
    }else if((mtvec & 0x3) == 1){
        mtvec = mtvec & 0xFFFFFFFC;
        // 判断是中断还是异常
        if(mcause & MCAUSE_INT){
            // 中断
            RISCV->pc = mtvec + 4 * (mcause & 0x7FFFFFFF);
        }else{
            // 异常
            RISCV->pc = mtvec&0xFFFFFFFD;
        }
    }
}

/**
 * @brief 返回指令
 */
void ret(struct riscv_Core *RISCV){
    uint32_t mstatus, mepc;
    mstatus = csr_read(RISCV->csr, CSR_MSTATUS);
    mepc = csr_read(RISCV->csr, CSR_MEPC);
    // 设置pc
    RISCV->pc = mepc;
    // 设置mstatus
    mstatus &= ~MSTATUS_MPIE;
    mstatus |= MSTATUS_MIE;
    csr_write(RISCV->csr, CSR_MSTATUS, mstatus);
}

/**
 * @brief 中断处理
 */
static void inter(struct riscv_Core *RISCV)
{
    uint32_t mstatus, mcause, mip, mie;
    mstatus = csr_read(RISCV->csr, CSR_MSTATUS);
    if ((mstatus & MSTATUS_MIE) == 0)
    {
        // 全局中断未使能
        return;
    }
    // 检查挂起的中断
    mip = csr_read(RISCV->csr, CSR_MIP);
    mie = csr_read(RISCV->csr, CSR_MIE);
    // 检查中断类型
    // 判断是否是外部中断
    if ((mip & MIP_MEIP) && (mie & MIE_MEIE))
    {
        // 外部中断
        mcause = 11;
        goto checkend;
    }
    // 判断是否是定时器中断
    if ((mip & MIP_MTIP) && (mie & MIE_MTIE))
    {
        // 定时器中断
        mcause = 7;
        goto checkend;
    }
    // 判断是否是软中断
    if ((mip & MIP_MSIP) && (mie & MIE_MSIE))
    {
        // 软中断
        mcause = 3;
        goto checkend;
    }
    // 没有中断
    return;
checkend:
    mcause |= MCAUSE_INT;
    trap(RISCV, mcause);
}

/**
 * @brief 取指令
 *
 * @param addr 地址
 * @return uint32_t 读取的值
 */
static uint32_t core_fetch(struct riscv_Core *RISCV)
{
    // 判断是否越界
    if ((RISCV->pc & 0xFF000000) != 0)
    {
        printf("取指令越界\n");
        return -1;
    }
    // 从ram中读取指令
    RISCV->inst = dev_read_w(RISCV->devices, RISCV->pc);
    return 0;
}

/**
 * @brief R-type指令
 *
 * @param RISCV 核心示例
 */
static void R_type(struct riscv_Core *RISCV)
{
    uint32_t rd, rs1, rs2, funct3, funct7;
    uint32_t inst = RISCV->inst;
    rd = (inst >> 7) & 0x1F;
    rs1 = (inst >> 15) & 0x1F;
    rs2 = (inst >> 20) & 0x1F;
    funct3 = (inst >> 12) & 0x7;
    funct7 = (inst >> 25) & 0x7F;
    switch (funct3)
    {
    case 0:
    {
        // add或sub - 加法或减法
        if (funct7 == 0)
        {
            RISCV->reg[rd] = RISCV->reg[rs1] + RISCV->reg[rs2]; // 将rs1和rs2相加结果存入rd
        }
        else if (funct7 == 0x20)
        {
            RISCV->reg[rd] = RISCV->reg[rs1] - RISCV->reg[rs2]; // 将rs1和rs2相减结果存入rd
        }
        else
        {
            printf("unknown funct7: %x\n", funct7);
        }
        break;
    }
    case 1:
    {
        // sll - 逻辑左移
        RISCV->reg[rd] = RISCV->reg[rs1] << (RISCV->reg[rs2] & 0x1F); // 将rs1逻辑左移rs2位结果存入rd
        break;
    }
    case 2:
    {
        // slt
        RISCV->reg[rd] = ((int32_t)RISCV->reg[rs1] < (int32_t)RISCV->reg[rs2]) ? 1 : 0; // 将rs1和rs2比较结果存入rd
        break;
    }
    case 3:
    {
        // sltu
        RISCV->reg[rd] = (RISCV->reg[rs1] < RISCV->reg[rs2]) ? 1 : 0; // 将rs1和rs2比较结果存入rd
        break;
    }
    case 4:
    {
        // xor - 异或
        RISCV->reg[rd] = RISCV->reg[rs1] ^ RISCV->reg[rs2]; // 将rs1和rs2异或结果存入rd
        break;
    }
    case 5:
    {
        // srl或sra - 逻辑右移或算术右移
        if (funct7 == 0)
        {
            RISCV->reg[rd] = RISCV->reg[rs1] >> (RISCV->reg[rs2] & 0x1F); // 将rs1逻辑右移rs2位结果存入rd
        }
        else if (funct7 == 0x20)
        {
            RISCV->reg[rd] = (int32_t)RISCV->reg[rs1] >> (RISCV->reg[rs2] & 0x1F); // 将rs1算术右移rs2位结果存入rd
        }
        else
        {
            printf("unknown funct7: %x\n", funct7);
        }
        break;
    }
    case 6:
    {
        // or - 或
        RISCV->reg[rd] = RISCV->reg[rs1] | RISCV->reg[rs2]; // 将rs1和rs2或结果存入rd
        break;
    }
    case 7:
    {
        // and - 与
        RISCV->reg[rd] = RISCV->reg[rs1] & RISCV->reg[rs2]; // 将rs1和rs2与结果存入rd
        break;
    }
    }
}

/**
 * @brief I-type指令 03 类型
 *
 * @param RISCV 核心示例
 */
static void I_type_03(struct riscv_Core *RISCV)
{
    uint32_t rd, rs1, imm, funct3;
    uint32_t inst = RISCV->inst;
    rd = (inst >> 7) & 0x1F;
    rs1 = (inst >> 15) & 0x1F;
    imm = (inst >> 20);
        if(imm & 0x800){
        imm |= 0xFFFFF000;
    }else{
        imm &= 0xFFF;
    }
    funct3 = (inst >> 12) & 0x7;
    switch (funct3)
    {
    case 0:
    {
        // lb 读取一个字节
        RISCV->reg[rd] = dev_read_b(RISCV->devices, RISCV->reg[rs1] + imm); // 读取rs1+imm处的一个字节存入rd
        break;
    }
    case 1:
    {
        // lh 读取两个字节
        RISCV->reg[rd] = dev_read_h(RISCV->devices, RISCV->reg[rs1] + imm); // 读取rs1+imm处的两个字节存入rd
        break;
    }
    case 2:
    {
        // lw 读取四个字节
        RISCV->reg[rd] = dev_read_w(RISCV->devices, RISCV->reg[rs1] + imm); // 读取rs1+imm处的四个字节存入rd
        break;
    }
    case 4:
    {
        // lbu 读取一个字节
        RISCV->reg[rd] = dev_read_b(RISCV->devices, RISCV->reg[rs1] + imm) & 0xFF; // 读取rs1+imm处的一个字节存入rd
        break;
    }
    case 5:
    {
        // lhu 读取两个字节
        RISCV->reg[rd] = dev_read_h(RISCV->devices, RISCV->reg[rs1] + imm) & 0xFFFF; // 读取rs1+imm处的两个字节存入rd
        break;
    }
    }
}

/**
 * @brief I-type指令 13 类型
 *
 * @param RISCV 核心示例
 */
static void I_type_13(struct riscv_Core *RISCV)
{
    uint32_t rd, rs1, funct3, funct7, shamt;
    int32_t imm;
    uint32_t inst = RISCV->inst;
    rd = (inst >> 7) & 0x1F;
    rs1 = (inst >> 15) & 0x1F;
    imm = (inst >> 20);
    if(imm & 0x800){
        imm |= 0xFFFFF000;
    }else{
        imm &= 0xFFF;
    }
    funct3 = (inst >> 12) & 0x7;
    shamt = (inst >> 20) & 0x1F;
    funct7 = (inst >> 25) & 0x7F;
    switch (funct3)
    {
    case 0:
    {
        // addi
        RISCV->reg[rd] = RISCV->reg[rs1] + imm; // 将rs1+imm结果存入rd
        break;
    }
    case 2:
    {
        // slti
        RISCV->reg[rd] = ((int32_t)RISCV->reg[rs1] < (int32_t)imm) ? 1 : 0; // 将rs1和imm比较结果存入rd
        break;
    }
    case 3:
    {
        // sltiu
        RISCV->reg[rd] = (RISCV->reg[rs1] < imm) ? 1 : 0; // 将rs1和imm比较结果存入rd
        break;
    }
    case 4:
    {
        // xori
        RISCV->reg[rd] = RISCV->reg[rs1] ^ imm; // 将rs1和imm异或结果存入rd
        break;
    }
    case 6:
    {
        // ori
        RISCV->reg[rd] = RISCV->reg[rs1] | imm; // 将rs1和imm或结果存入rd
        break;
    }
    case 7:
    {
        // andi
        RISCV->reg[rd] = RISCV->reg[rs1] & imm; // 将rs1和imm与结果存入rd
        break;
    }
    case 1:
    {
        // slli
        RISCV->reg[rd] = RISCV->reg[rs1] << shamt; // 将rs1逻辑左移shamt位结果存入rd
        break;
    }
    case 5:
    {
        if (funct7 == 0)
        {
            // srli
            RISCV->reg[rd] = RISCV->reg[rs1] >> shamt; // 将rs1逻辑右移shamt位结果存入rd
        }
        else if (funct7 == 0x20)
        {
            // srai
            RISCV->reg[rd] = (int32_t)RISCV->reg[rs1] >> shamt; // 将rs1算术右移shamt位结果存入rd
        }
        else
        {
            printf("unknown funct7: %x\n", funct7);
        }
        break;
    }
    }
}

/**
 * @brief S-type指令
 *
 * @param RISCV 核心示例
 */
static void S_type(struct riscv_Core *RISCV)
{
    uint32_t rs1, rs2, funct3;
    int32_t imm;
    uint32_t inst = RISCV->inst;
    rs1 = (inst >> 15) & 0x1F;
    rs2 = (inst >> 20) & 0x1F;
    imm = ((inst >> 25) << 5) | ((inst >> 7) & 0x1F);
    if(imm & 0x800){
        imm |= 0xFFFFF000;
    }else{
        imm &= 0xFFF;
    }
    funct3 = (inst >> 12) & 0x7;
    switch (funct3)
    {
    case 0:
    {
        // sb
        dev_write_b(RISCV->devices, RISCV->reg[rs1] + imm, RISCV->reg[rs2]); // 将rs2写入rs1+imm处
        break;
    }
    case 1:
    {
        // sh
        dev_write_h(RISCV->devices, RISCV->reg[rs1] + imm, RISCV->reg[rs2]); // 将rs2写入rs1+imm处
        break;
    }
    case 2:
    {
        // sw
        dev_write_w(RISCV->devices, RISCV->reg[rs1] + imm, RISCV->reg[rs2]); // 将rs2写入rs1+imm处
        break;
    }
    }
}

/**
 * @brief B-type指令
 *
 * @param RISCV 核心示例
 */
static int B_type(struct riscv_Core *RISCV)
{
    uint32_t rs1, rs2, funct3;
    uint32_t inst = RISCV->inst;
    int32_t imm;
    int ret = 0;
    rs1 = (inst >> 15) & 0x1F;
    rs2 = (inst >> 20) & 0x1F;
    imm = (((inst >> 7) & 0x1) << 11) | ((inst >> 25 & 0x3F) << 5) | (((inst >> 8) & 0xF) << 1);
    // imm = ((inst >> 31) << 12) ? (0xFFFFFFFF & imm): imm;
    if ((inst >> 31) & 0x1)
    {
        imm = 0xFFFFF000 | imm;
    }
    funct3 = (inst >> 12) & 0x7;
    switch (funct3)
    {
    case 0:
    {
        // beq
        if (RISCV->reg[rs1] == RISCV->reg[rs2])
        {
            RISCV->pc += imm; // pc跳转imm
            ret = 1;
        }
        break;
    }
    case 1:
    {
        // bne
        if (RISCV->reg[rs1] != RISCV->reg[rs2])
        {
            RISCV->pc += imm; // pc跳转imm
            ret = 1;
        }
        break;
    }
    case 4:
    {
        // blt
        if ((int32_t)RISCV->reg[rs1] < (int32_t)RISCV->reg[rs2])
        {
            RISCV->pc += imm; // pc跳转imm
            ret = 1;
        }
        break;
    }
    case 5:
    {
        // bge
        if ((int32_t)RISCV->reg[rs1] >= (int32_t)RISCV->reg[rs2])
        {
            RISCV->pc += imm; // pc跳转imm
            ret = 1;
        }
        break;
    }
    case 6:
    {
        // bltu
        if (RISCV->reg[rs1] < RISCV->reg[rs2])
        {
            RISCV->pc += imm; // pc跳转imm
            ret = 1;
        }
        break;
    }
    case 7:
    {
        // bgeu
        if (RISCV->reg[rs1] >= RISCV->reg[rs2])
        {
            RISCV->pc += imm; // pc跳转imm
            ret = 1;
        }
        break;
    }
    }
    return ret;
}

/**
 * @brief U-type指令
 *
 * @param RISCV 核心示例
 */
static void LUI(struct riscv_Core *RISCV)
{
    uint32_t inst = RISCV->inst;
    uint32_t rd, imm;
    rd = (inst >> 7) & 0x1F;
    imm = inst & 0xFFFFF000;
    RISCV->reg[rd] = imm; // 将imm存入rd
}

/**
 * @brief U-type指令
 *
 * @param RISCV 核心示例
 */
static void AUIPC(struct riscv_Core *RISCV)
{
    uint32_t rd, imm;
    uint32_t inst = RISCV->inst;
    rd = (inst >> 7) & 0x1F;
    imm = inst & 0xFFFFF000;
    RISCV->reg[rd] = RISCV->pc + imm; // 将pc+imm存入rd
}

/**
 * @brief 无条件跳转相对于寄存器
 *
 * @param RISCV 核心示例
 */
static uint32_t JALR(struct riscv_Core *RISCV)
{
    uint32_t inst = RISCV->inst;
    uint32_t rd, rs1, imm;
    rd = (inst >> 7) & 0x1F;
    rs1 = (inst >> 15) & 0x1F;
    imm = (inst >> 20);
    RISCV->reg[rd] = RISCV->pc + 4;                   // 将pc+4存入rd
    RISCV->pc = (RISCV->reg[rs1] + imm) & 0xFFFFFFFE; // 将rs1+imm存入pc
    return 0;
}

static uint32_t JAL(struct riscv_Core *RISCV)
{
    // 31|30|29|28|27|26|25|24|23|22|21|20|19|18|17|16|15|14|13|12|11|10|9|8|7|6|5|4|3|2|1|0
    // 20|10|9 |8 |7 |6 |5 |4 |3 |2 |1 |11|19|18|17|16|15|14|13|12|-12-
    uint32_t rd;
    int32_t imm;
    uint32_t inst = RISCV->inst;
    rd = (inst >> 7) & 0x1F;
    imm = ((inst >> 12) & 0xFF) | ((inst >> 9) & 0x800) | ((inst >> 20) & 0x7FE);
    if ((inst >> 31) & 0x1)
    {
        imm = 0xFFF00000 | imm;
    }
    RISCV->reg[rd] = RISCV->pc + 4; // 将pc+4存入rd
    RISCV->pc += imm;               // pc跳转imm
    return 0;
}

/**
 * @brief csr寄存器操作
 */
static int system_73(struct riscv_Core *RISCV)
{
    uint32_t inst = RISCV->inst;
    uint32_t rd, rs1, csr, fun3;
    uint32_t temp;
    rd = (inst >> 7) & 0x1F;
    rs1 = (inst >> 15) & 0x1F;
    csr = (inst >> 20) & 0xFFF;
    fun3 = (inst >> 12) & 0x7;
    switch (fun3)
    {
    case 0:
        if(csr == 0x302){
            // mret
            ret(RISCV);
            return 1;
        }
        // ecall
        break;
    case 1:
        // csrrw    CSR寄存器读后写。记csr值为t，将rs1写入csr，再将t写入rd。
        temp = csr_read(RISCV->csr, csr);
        csr_write(RISCV->csr, csr, RISCV->reg[rs1]);
        RISCV->reg[rd] = temp;
        break;
    case 2:
        // csrrs    CSR寄存器读后置位。记csr值为t，将t和rs1的按位或结果写入csr，再将t 写入rd。
        temp = csr_read(RISCV->csr, csr);
        csr_write(RISCV->csr, csr, temp | RISCV->reg[rs1]);
        RISCV->reg[rd] = temp;
        break;
    case 3:
        // csrrc   CSR寄存器读后清位。记csr值为t，将rs1的反码和t按位与，结果写入csr，再将t写入rd。
        temp = csr_read(RISCV->csr, csr);
        csr_write(RISCV->csr, csr, temp & ~RISCV->reg[rs1]);
        RISCV->reg[rd] = temp;
        break;
    case 5:
        // csrrwi   CSR寄存器读后写立即数。将csr 的值复制到rd 中，再将5 位立即数zimm 的零扩展结果写入csr。
        temp = csr_read(RISCV->csr, csr);
        csr_write(RISCV->csr, csr, rs1);
        RISCV->reg[rd] = temp;
        break;
    case 6:
        // csrrsi   CSR寄存器读后置位立即数。记csr 的值为t，将5 位立即数zimm 零扩展后，和t 按位或，结果写入csr，再将t 写入rd（csr 中的第5 及更高的位不变）
        temp = csr_read(RISCV->csr, csr);
        csr_write(RISCV->csr, csr, temp | rs1);
        RISCV->reg[rd] = temp;
        break;
    case 7:
        // csrrci  CSR寄存器读后清位立即数。记csr 的值为t，将5 位立即数zimm 零扩展后的反码和t 按位与，结果写入csr，再将t 写入rd（csr 中的第5 及更高的位不变）。
        temp = csr_read(RISCV->csr, csr);
        csr_write(RISCV->csr, csr, temp & ~rs1);
        RISCV->reg[rd] = temp;
        break;
    }
    return 0;
}

/**
 * @brief 执行指令
 *
 * @param RISCV 核心示例
 */
void run_inst(struct riscv_Core *RISCV)
{
    uint32_t opcode;
    uint32_t inst;
    // 检查是否有中断
    inter(RISCV);
    // 取指令
    if (core_fetch(RISCV))
    {
        return;
    }
    // 解析执行指令
    inst = RISCV->inst;
    opcode = inst & 0x7F; // 取低7位, 即opcode, 用于判断指令类型
    switch (opcode)
    {
    case 0x33:
    {
        // R-type
        R_type(RISCV);
        break;
    }
    case 0x03:
    {
        // I-type
        I_type_03(RISCV);
        break;
    }
    case 0x67:
    {
        // JALR - 无条件跳转
        JALR(RISCV);
        goto jump;
        break;
    }
    case 0x13:
    {
        // I-type
        I_type_13(RISCV);
        break;
    }
    case 0x23:
    {
        // S-type
        S_type(RISCV);
        break;
    }
    case 0x63:
    {
        // B-type
        if (B_type(RISCV) == 1)
        {
            goto jump;
        }
        break;
    }
    case 0x37:
    {
        // U-type
        LUI(RISCV);
        break;
    }
    case 0x17:
    {
        // UJ-type
        AUIPC(RISCV);
        break;
    }
    case 0x6F:
    {
        // JAL - 无条件跳转
        JAL(RISCV);
        goto jump;
        break;
    }
    case 0x1F:
    {
        // FENCE

        break;
    }
    case 0x73:
    {
        // SYSTEM
        if(system_73(RISCV)){
            goto jump;
        }
        break;
    }
    default:
    {
        printf("unknown opcode: %x\n", opcode);
        break;
    }
    }
    // 执行结束，移动程序计数器
    // pc+4
    RISCV->pc += 4;
    RISCV->reg[0] = 0; // x0始终为0
    return;
jump:
    // pc跳转, 所以不需要pc+4
    RISCV->reg[0] = 0; // x0始终为0
    return;
}
